using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Net;
using System.Drawing;
using System.Drawing.Imaging;
using W3C.Soap;
using System.Linq;
using System.Windows.Forms;
using System.Threading;
using System.Globalization;
using System.Diagnostics;

// CCR & DSS Namespaces
using Microsoft.Ccr.Core;
using Microsoft.Ccr.Adapters.WinForms;
using Microsoft.Dss.Core.Attributes;
using Microsoft.Dss.ServiceModel.Dssp;
using Microsoft.Dss.ServiceModel.DsspServiceBase;
using Microsoft.Dss.Core.DsspHttp;

//Namespaces with local nickname
using dssp = Microsoft.Dss.ServiceModel.Dssp;
using submgr = Microsoft.Dss.Services.SubscriptionManager;

//Simulation
//using simjaguar = SimJaguar.Jaguar.Proxy; // For one jaguar use.
using simjaguars = SimJaguars.Proxy;
using Microsoft.Robotics.Simulation.Engine;
using Microsoft.Robotics.PhysicalModel;
using xnaTypes = Microsoft.Xna.Framework;
using Microsoft.Robotics.Simulation.Physics;


namespace SimJaguarsControl
{
    [Contract(Contract.Identifier)]
    [DisplayName("SimJaguarsControl")]
    [Description("SimJaguarsControl service (no description provided)")]
    class SimJaguarsControlService : DsspServiceBase
    {
        #region Service Pre-definitions

        #region Variables
        MainForm mainUI;

        private static Mapper[] _map = new Mapper[4];
        Bitmap[] _mapBitmap = new Bitmap[4];

        const float PastMark = 0.50f;  //meters
        Dictionary<string, int> _stringToIntMapping = new Dictionary<string, int>(); //To keep track on waypoints entities

        CameraEntity _observer;
        Port<Bitmap> _bitmapPort;

        SimulatorConfiguration _defaultConfig = null;

        // This port receives events from the user interface
        FromWinformEvents _fromWinformPort = new FromWinformEvents();
        
        private const string root = "C:\\Program Files (x86)\\Microsoft Robotics Dev Studio 2008 R3\\jcepeda\\CS\\SimJaguarsControl\\SimJaguarsControl\\SimJaguarsControl\\";

        Image[] sloadedImage1 = new Image[4];
        Image[] sloadedImage2 = new Image[4];
        Image[] sloadedImage3 = new Image[4];

        //For Drawings
        private double[] PreYaw = new double[4] { 0, 0, 0, 0 };
        private double[] PreRoll = new double[4] { 0, 0, 0, 0 };
        private double[] PrePitch = new double[4] { 0, 0, 0, 0 };

        // ONE JAGUAR PREVIOUS CODE
        //public simjaguar.UpdateSuccessMsgRequest Speed;
        simjaguars.UpdateSuccessMsgRequest Speed;

        bool firstTime = true;

        ////Number of robots
        //static int numRobots = 1;

        ////Robot parameters
        //static double WheelRadius = 0.137;
        ////static double VehicleLength = 0.40;
        //static double VehicleWidth = 0.426;

        ////Wheels Speed (RPM's)
        //static int MaxSpeed = 1;
        //double LeftWheelSpeed = 0;
        //double RightWheelSpeed = 0;
        #endregion        

        #region Ports
        /// <summary>
        /// Main service port
        /// </summary>
        [ServicePort("/SimJaguarsControl", AllowMultipleInstances = true)]
        SimJaguarsControlOperations _mainPort = new SimJaguarsControlOperations();

        [SubscriptionManagerPartner]
        submgr.SubscriptionManagerPort _submgrPort = new submgr.SubscriptionManagerPort();

        //ONE JAGUAR PREVIOUS CODE
        ///// <summary>
        ///// SimJaguarService partner
        ///// </summary>
        //[Partner("SimJaguar", Contract = simjaguar.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        //simjaguar.JaguarOperations _simjaguarPort = new simjaguar.JaguarOperations();
        //simjaguar.JaguarOperations _simjaguarNotify = new simjaguar.JaguarOperations();
        
        /// <summary>
        /// SimJaguarsService partner
        /// </summary>
        [Partner("SimJaguars", Contract = simjaguars.Contract.Identifier, CreationPolicy = PartnerCreationPolicy.UseExisting)]
        simjaguars.SimJaguarsOperations _simjaguarsPort = new simjaguars.SimJaguarsOperations();
        simjaguars.SimJaguarsOperations _simjaguarsNotify = new simjaguars.SimJaguarsOperations();

        SimulationEnginePort _notificationTarget = new SimulationEnginePort();
        private SimulationEnginePort _simEnginePort = SimulationEngine.GlobalInstancePort;
        #endregion

        #region State and Constructor
        [ServiceState]
        SimJaguarsControlState _state;

        public SimJaguarsControlService(DsspServiceCreationPort creationPort)
            : base(creationPort)
        {
        }
        #endregion

        #endregion

        /// Service start
        protected override void Start()
        {
            #region Initialization
            _state = new SimJaguarsControlState();
            _state.Initialize();
            #endregion

            base.Start();

            #region Simulator Config
            EntitySubscribeRequestType req = new EntitySubscribeRequestType();
            req.Name = "MainCamera";
            SimulationEngine.GlobalInstancePort.Subscribe(req, _notificationTarget);

            // Set the Simulator camera view and resolution
            UpdateCameraView view = new UpdateCameraView(new CameraView());
            view.Body.EyePosition = new Vector3(1, 5, 1);
            view.Body.LookAtPoint = new Vector3(0, 0, 0);
            view.Body.XResolution = 640;
            view.Body.YResolution = 480;
            SimulationEngine.GlobalInstancePort.Post(view);

            // get the current simulator configuration
            _defaultConfig = new SimulatorConfiguration(true);
            #endregion

            #region Embedded Simulation
            Activate(Arbiter.Choice(SimulationEngine.GlobalInstancePort.Query(_defaultConfig),
                delegate(SimulatorConfiguration config)
                {
                    _defaultConfig = config;
                    if (mainUI != null)
                        WinFormsServicePort.FormInvoke(delegate() { mainUI.SetHeadless(config.Headless); });
                },
                delegate(W3C.Soap.Fault fault)
                {
                }
            ));
            #endregion

            #region Winform Messaging
            // Add the winform message handler to the interleave
            Activate(Arbiter.Interleave(
                new TeardownReceiverGroup(),
                new ExclusiveReceiverGroup
                (
                    Arbiter.Receive<InsertSimulationEntity>(false, _notificationTarget, InsertEntityNotificationHandlerFirstTime),
                    Arbiter.Receive<FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler)
                ),
                new ConcurrentReceiverGroup()
            ));
            #endregion

            MakeForm();

            #region SimJaguars Messaging
            //ONE JAGUAR PREVIOUS CODE
            //Activate<ITask>(Arbiter.Receive<simjaguar.Replace>(true, _simjaguarNotify, NotifySimJaguarHandler));
            //_simjaguarPort.Subscribe(_simjaguarNotify);

            Activate<ITask>(Arbiter.Receive<simjaguars.Replace>(true, _simjaguarsNotify, NotifySimJaguarsHandler));
            _simjaguarsPort.Subscribe(_simjaguarsNotify);
            #endregion

            #region DSS
            MainPortInterleave.CombineWith(
                Arbiter.Interleave(
                new TeardownReceiverGroup(),
                new ExclusiveReceiverGroup(),
                new ConcurrentReceiverGroup(Arbiter.Receive<Get>(true, _mainPort, GetHandler),
                Arbiter.Receive<dssp.DsspDefaultLookup>(true, _mainPort, DefaultLookupHandler)
                ))
            );
            #endregion

        }

        #region Display Operations
        bool MakeForm()
        {
            Fault fault = null;
            RunForm runForm = new RunForm(CreateMainForm);

            WinFormsServicePort.Post(runForm);

            Arbiter.Choice(
                runForm.pResult,
                delegate(SuccessResult success) { },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                }
            );

            if (fault != null)
            {
                LogError(null, "Failed to create Main Form", fault);
                return false;
            }
            else
            {
                return true;
            }

        }
        Form CreateMainForm()
        {
            mainUI = new MainForm(_fromWinformPort);
            return mainUI;
        }
        private void loadIMUFigures()
        {
            sloadedImage1[1] = Image.FromFile(root + "Resources\\Frontal.jpg");
            mainUI.sPictureBoxRoll.Image = sloadedImage1[1];
            mainUI.sPictureBoxRoll.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage2[1] = Image.FromFile(root + "Resources\\Superior.jpg");
            mainUI.sPictureBoxYaw.Image = sloadedImage2[1];
            mainUI.sPictureBoxYaw.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage3[1] = Image.FromFile(root + "Resources\\Lateral.jpg");
            mainUI.sPictureBoxPitch.Image = sloadedImage3[1];
            mainUI.sPictureBoxPitch.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage1[2] = Image.FromFile(root + "Resources\\Frontal.jpg");
            mainUI.sPictureBoxRoll2.Image = sloadedImage1[2];
            mainUI.sPictureBoxRoll2.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage2[2] = Image.FromFile(root + "Resources\\Superior.jpg");
            mainUI.sPictureBoxYaw2.Image = sloadedImage2[2];
            mainUI.sPictureBoxYaw2.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage3[2] = Image.FromFile(root + "Resources\\Lateral.jpg");
            mainUI.sPictureBoxPitch2.Image = sloadedImage3[2];
            mainUI.sPictureBoxPitch2.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage1[3] = Image.FromFile(root + "Resources\\Frontal.jpg");
            mainUI.sPictureBoxRoll3.Image = sloadedImage1[3];
            mainUI.sPictureBoxRoll3.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage2[3] = Image.FromFile(root + "Resources\\Superior.jpg");
            mainUI.sPictureBoxYaw3.Image = sloadedImage2[3];
            mainUI.sPictureBoxYaw3.SizeMode = PictureBoxSizeMode.CenterImage;

            sloadedImage3[3] = Image.FromFile(root + "Resources\\Lateral.jpg");
            mainUI.sPictureBoxPitch3.Image = sloadedImage3[3];
            mainUI.sPictureBoxPitch3.SizeMode = PictureBoxSizeMode.CenterImage;

        }
        public void UpdateIMUFigures()
        {
            for (int i = 1; i < 4; i++)
            {
                if (sloadedImage1[i] != null && sloadedImage2[i] != null && sloadedImage3[i] != null)
                {
                    if (i == 1 && mainUI.SimJaguar1IMUDisplay.Checked)
                    {
                        if (Math.Abs(_state.SimJaguar[i].Pose.Yaw - PreYaw[i]) > 3)
                        {
                            mainUI.sPictureBoxYaw.Image = mainUI.RotateImage(sloadedImage2[i], -_state.SimJaguar[i].Pose.Yaw);
                            mainUI.sPictureBoxYaw.Refresh();
                            PreYaw[i] = _state.SimJaguar[i].Pose.Yaw;
                        }
                        if (Math.Abs(_state.SimJaguar[i].Pose.Roll - PreRoll[i]) > 3)
                        {
                            mainUI.sPictureBoxRoll.Image = mainUI.RotateImage(sloadedImage1[i], -_state.SimJaguar[i].Pose.Roll);
                            mainUI.sPictureBoxRoll.Refresh();
                            PreRoll[i] = _state.SimJaguar[i].Pose.Roll;
                        }
                        if (Math.Abs(_state.SimJaguar[i].Pose.Pitch - PrePitch[i]) > 3)
                        {
                            mainUI.sPictureBoxPitch.Image = mainUI.RotateImage(sloadedImage3[i], -_state.SimJaguar[i].Pose.Pitch);
                            mainUI.sPictureBoxPitch.Refresh();
                            PrePitch[i] = _state.SimJaguar[i].Pose.Pitch;
                        }
                    }
                    else if (i == 2 && mainUI.SimJaguar2IMUDisplay.Checked)
                    {
                        if (Math.Abs(_state.SimJaguar[i].Pose.Yaw - PreYaw[i]) > 3)
                        {
                            mainUI.sPictureBoxYaw2.Image = mainUI.RotateImage(sloadedImage2[i], -_state.SimJaguar[i].Pose.Yaw);
                            mainUI.sPictureBoxYaw2.Refresh();
                            PreYaw[i] = _state.SimJaguar[i].Pose.Yaw;
                        }
                        if (Math.Abs(_state.SimJaguar[i].Pose.Roll - PreRoll[i]) > 3)
                        {
                            mainUI.sPictureBoxRoll2.Image = mainUI.RotateImage(sloadedImage1[i], -_state.SimJaguar[i].Pose.Roll);
                            mainUI.sPictureBoxRoll2.Refresh();
                            PreRoll[i] = _state.SimJaguar[i].Pose.Roll;
                        }
                        if (Math.Abs(_state.SimJaguar[i].Pose.Pitch - PrePitch[i]) > 3)
                        {
                            mainUI.sPictureBoxPitch2.Image = mainUI.RotateImage(sloadedImage3[i], -_state.SimJaguar[i].Pose.Pitch);
                            mainUI.sPictureBoxPitch2.Refresh();
                            PrePitch[i] = _state.SimJaguar[i].Pose.Pitch;
                        }
                    }
                    else if (i == 3 && mainUI.SimJaguar3IMUDisplay.Checked)
                    {
                        if (Math.Abs(_state.SimJaguar[i].Pose.Yaw - PreYaw[i]) > 3)
                        {
                            mainUI.sPictureBoxYaw3.Image = mainUI.RotateImage(sloadedImage2[i], -_state.SimJaguar[i].Pose.Yaw);
                            mainUI.sPictureBoxYaw3.Refresh();
                            PreYaw[i] = _state.SimJaguar[i].Pose.Yaw;
                        }
                        if (Math.Abs(_state.SimJaguar[i].Pose.Roll - PreRoll[i]) > 3)
                        {
                            mainUI.sPictureBoxRoll3.Image = mainUI.RotateImage(sloadedImage1[i], -_state.SimJaguar[i].Pose.Roll);
                            mainUI.sPictureBoxRoll3.Refresh();
                            PreRoll[i] = _state.SimJaguar[i].Pose.Roll;
                        }
                        if (Math.Abs(_state.SimJaguar[i].Pose.Pitch - PrePitch[i]) > 3)
                        {
                            mainUI.sPictureBoxPitch3.Image = mainUI.RotateImage(sloadedImage3[i], -_state.SimJaguar[i].Pose.Pitch);
                            mainUI.sPictureBoxPitch3.Refresh();
                            PrePitch[i] = _state.SimJaguar[i].Pose.Pitch;
                        }
                    }
                }
            }
        }
        IEnumerator<ITask> UpdateMap()
        {
            for (int robotID = 1; robotID < 4; robotID++)
            {
                // Insurance in case the LRF is not behaving ...
                if (_state.SimJaguar[robotID].Sensors.laserData.DistanceMeasurements == null)
                    yield break;

                // Create a new map if we don't have one
                if (_map[robotID] == null)
                {
                    double startAngle = _state.SimJaguar[robotID].Sensors.laserData.AngularRange / 2.0;
                    // AngularResolution is zero for the simulated LRF!
                    // This is a bug that I have reported and it should be
                    // fixed in V1.5. In the meantime, calculate the value.
                    double angleIncrement = (double)_state.SimJaguar[robotID].Sensors.laserData.AngularRange / (double)_state.SimJaguar[robotID].Sensors.laserData.DistanceMeasurements.Length;
                    // Create the mapper object
                    _map[robotID] = new Mapper();
                    // Set the drawing mode
                    _map[robotID].mode = _state.SimJaguar[robotID].MAP.DrawingMode;
                    // Set the parameters and allocate the necessary memory
                    _mapBitmap[robotID] = _map[robotID].Init(_state.SimJaguar[robotID].Sensors.laserData.DistanceMeasurements.Length, _state.SimJaguar[robotID].MAP.MapMaxRange,
                                    startAngle, angleIncrement,
                                    _state.SimJaguar[robotID].MAP.MapWidth, _state.SimJaguar[robotID].MAP.MapHeight,
                                    _state.SimJaguar[robotID].MAP.MapResolution);
                    // Clear the map initially (could be moved to Alloc)
                    _map[robotID].Clear();
                }

                // Add new laser data to the robotID map, offsets help to center map image.
                int offsetX = 12;
                int offsetZ = 6;
                _map[robotID].Add(-(float)_state.SimJaguar[robotID].Pose.X_pose - offsetX, (float)_state.SimJaguar[robotID].Pose.Z_pose + offsetZ, ((float)_state.SimJaguar[robotID].Pose.Yaw - 90), _state.SimJaguar[robotID].Sensors.laserData.DistanceMeasurements.Length, _state.SimJaguar[robotID].Sensors.laserData.DistanceMeasurements);


                // Display the new map by requesting a bitmap image and then
                // passing it to the code inside the Windows Form; first for robotID map and then to Team map
                if (_map[robotID].MapToImage() != null && mainUI.drawMaps.Checked)
                    DisplayMap(_map[robotID].MapToImage(), robotID);
            }

        }
        bool DisplayMap(Bitmap bmp, int robotID)
        {
            Fault fault = null;

            FormInvoke setImage = new FormInvoke(
                delegate()
                {
                    if (bmp != null && mainUI != null)
                    {
                        if (robotID == 1)
                        {
                            mainUI.MapImage = bmp;
                        }
                        else if (robotID == 2)
                        {
                            mainUI.MapImage2 = bmp;
                        }
                        else if (robotID == 3)
                        {
                            mainUI.MapImage3 = bmp;
                        }
                    }
                }
            );

            WinFormsServicePort.Post(setImage);

            Arbiter.Choice(
                setImage.ResultPort,
                delegate(EmptyValue success) { },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                }
            );

            if (fault != null)
            {
                LogError(null, "Unable to set Map image on form", fault);
                return false;
            }
            else
            {
                return true;
            }
        }
        /// <param name="Waypoints">The list of the waypoints to be updated.</param>
        /// <returns>A Tuple referring if the point is new, and the index of the augmented waypoint if not new</returns>
        private Tuple<bool, int> UpdateWaypoints(int robotID, List<Points> Waypoints)
        {
            bool newWaypoint = false;
            int index = 0;
            double near_dist = 1000;
            int near_index = 0;
            double distance = 0;
            SimJaguarsControl.Points newPoint = new SimJaguarsControl.Points();
            for (int i = 0; i < Waypoints.Count; i++)
            {
                newWaypoint = false;  //Quick reset of newWaypoint for avoiding the CCR to win the trigger of the added point

                #region Determine Nearest Waypoint
                distance = Math.Abs(CalculateDistance(Waypoints[i].X, Waypoints[i].Z, robotID));
                //Determining which is the nearest waypoint to robot's current pose
                if (distance < near_dist)
                {
                    near_dist = distance;
                    near_index = i;
                }
                #endregion

                #region Determine if we are inside an existing waypoint
                //If the waypoint we are checking is inside our PastMark region then increase its count.
                if ((_state.SimJaguar[robotID].Pose.X_pose - PastMark <= Waypoints[i].X) &&
                    (_state.SimJaguar[robotID].Pose.X_pose + PastMark >= Waypoints[i].X) &&
                    (_state.SimJaguar[robotID].Pose.Z_pose - PastMark <= Waypoints[i].Z) &&
                    (_state.SimJaguar[robotID].Pose.Z_pose + PastMark >= Waypoints[i].Z))
                {
                    Waypoints[i].Steps++;
                    //If we increased the count there this is not a newWaypoint.
                    //newWaypoint = false;
                    index = i;
                    //We can break now
                    i = Waypoints.Count;
                }
                else
                    newWaypoint = true;
                #endregion
            }
            //If this is a new waypoint, then we must add it.
            if (newWaypoint || Waypoints.Count == 0)
            {
                //If we are close to an existing waypoint then we avoid an overlap adding the new point according to                                the waypoint we just left.
                if (Waypoints.Count != 0 && near_dist < 3 * PastMark)
                {
                    //Maybe we should change the (int) to (int)Math.Round(..)
                    #region Waypoint Side Evaluation
                    if (_state.SimJaguar[robotID].Pose.X_pose <= Waypoints[near_index].X - PastMark &&
                        _state.SimJaguar[robotID].Pose.Z_pose <= Waypoints[near_index].Z - PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X - 2 * PastMark);
                        newPoint.Z = (int)(Waypoints[near_index].Z - 2 * PastMark);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.X_pose >= Waypoints[near_index].X + PastMark &&
                        _state.SimJaguar[robotID].Pose.Z_pose >= Waypoints[near_index].Z + PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X + 2 * PastMark);
                        newPoint.Z = (int)(Waypoints[near_index].Z + 2 * PastMark);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.X_pose <= Waypoints[near_index].X - PastMark &&
                        _state.SimJaguar[robotID].Pose.Z_pose >= Waypoints[near_index].Z + PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X - 2 * PastMark);
                        newPoint.Z = (int)(Waypoints[near_index].Z + 2 * PastMark);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.X_pose >= Waypoints[near_index].X + PastMark &&
                        _state.SimJaguar[robotID].Pose.Z_pose <= Waypoints[near_index].Z - PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X + 2 * PastMark);
                        newPoint.Z = (int)(Waypoints[near_index].Z - 2 * PastMark);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.X_pose <= Waypoints[near_index].X - PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X - 2 * PastMark);
                        newPoint.Z = (int)(Waypoints[near_index].Z);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.X_pose >= Waypoints[near_index].X + PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X + 2 * PastMark);
                        newPoint.Z = (int)(Waypoints[near_index].Z);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.Z_pose <= Waypoints[near_index].Z - PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X);
                        newPoint.Z = (int)(Waypoints[near_index].Z - 2 * PastMark);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    else if (_state.SimJaguar[robotID].Pose.Z_pose >= Waypoints[near_index].Z + PastMark)
                    {
                        newPoint.X = (int)(Waypoints[near_index].X);
                        newPoint.Z = (int)(Waypoints[near_index].Z + 2 * PastMark);
                        newPoint.Steps = 0;
                        Waypoints.Add(newPoint);
                    }
                    #endregion
                }
                //If far from every waypoint then add the point in the new position according to robot pose.
                else
                {
                    newPoint.X = (int)Math.Round(_state.SimJaguar[robotID].Pose.X_pose);
                    newPoint.Z = (int)Math.Round(_state.SimJaguar[robotID].Pose.Z_pose);
                    newPoint.Steps = 0;
                    Waypoints.Add(newPoint);
                }

                #region Display the "Cookie-Path"
                if (mainUI.drawWaypoints.Checked)
                {
                    BoxShapeProperties cBoxShape = new BoxShapeProperties();
                    cBoxShape.Dimensions = new Vector3(2 * PastMark, 0.0001f, 2 * PastMark);  //Size according to PastMark
                    if (robotID == 1)
                        cBoxShape.DiffuseColor = new Vector4(20, 0, 0, 0); //RED
                    else if (robotID == 2)
                        cBoxShape.DiffuseColor = new Vector4(0, 20, 0, 0); //GREEN
                    else if (robotID == 3)
                        cBoxShape.DiffuseColor = new Vector4(0, 0, 20, 0); //BLUE
                    SingleShapeEntity box = new SingleShapeEntity();
                    box = new SingleShapeEntity(new BoxShape(cBoxShape),
                    new Vector3((float)_state.SimJaguar[robotID].Waypoints.Points[_state.SimJaguar[robotID].Waypoints.Points.Count - 1].X,
                            0.009f,
                            (float)_state.SimJaguar[robotID].Waypoints.Points[_state.SimJaguar[robotID].Waypoints.Points.Count - 1].Z));
                    // Name the entity. All entities must have unique names
                    box.State.Name = GetStringWithUniquePostfix("Point");
                    // Insert entity in simulation.  
                    _simEnginePort.Insert(box);
                }
                #endregion

            }
            Tuple<bool, int> result = new Tuple<bool, int>(newWaypoint, index);
            return result;
        }
        void UpdateWaypoints()
        {
            #region UpdateWaypoints
            DateTime CurrentTime = DateTime.Now;
            if (CurrentTime.Subtract(_state.SimJaguar[1].Waypoints.LastTime) >
                System.TimeSpan.FromSeconds(Math.Sqrt(PastMark * PastMark * 2) * 1000 / 500)) //500 is mm/s speed
            {
                _state.SimJaguar[1].Waypoints.LastTime = DateTime.Now;
                _state.SimJaguar[1].Waypoints.LastPoint = UpdateWaypoints(1, _state.SimJaguar[1].Waypoints.Points);
            }
            CurrentTime = DateTime.Now;
            if (CurrentTime.Subtract(_state.SimJaguar[2].Waypoints.LastTime) >
                System.TimeSpan.FromSeconds(Math.Sqrt(PastMark * PastMark * 2) * 1000 / 500)) //500 is mm/s speed
            {
                _state.SimJaguar[2].Waypoints.LastTime = DateTime.Now;
                _state.SimJaguar[2].Waypoints.LastPoint = UpdateWaypoints(2, _state.SimJaguar[2].Waypoints.Points);
            }
            CurrentTime = DateTime.Now;
            if (CurrentTime.Subtract(_state.SimJaguar[3].Waypoints.LastTime) >
                System.TimeSpan.FromSeconds(Math.Sqrt(PastMark * PastMark * 2) * 1000 / 500)) //500 is mm/s speed
            {
                _state.SimJaguar[3].Waypoints.LastTime = DateTime.Now;
                _state.SimJaguar[3].Waypoints.LastPoint = UpdateWaypoints(3, _state.SimJaguar[3].Waypoints.Points);
            }
            #endregion;

        }
        #endregion

        #region Handlers
        #region DSS
        /// <summary>
        /// Handles Subscribe messages
        /// </summary>
        /// <param name="subscribe">the subscribe request</param>
        [ServiceHandler]
        public void SubscribeHandler(Subscribe subscribe)
        {
            SubscribeHelper(_submgrPort, subscribe.Body, subscribe.ResponsePort);
        }
        void GetHandler(Get get)
        {
            get.ResponsePort.Post(_state);
        }
        /// <summary> Replace Handler
        /// The Replace Handler is the essential handler for sending a notification every time there is a change in the Service 
        /// State.
        /// </summary>
        /// <param name="replace"></param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Exclusive)]
        public IEnumerator<ITask> ReplaceHandler(Replace replace)
        {
            //Add here the actions in response to the replace notification   

            _state = replace.Body;
            SendNotification<Replace>(_submgrPort, replace.Body);
            replace.ResponsePort.Post(new DefaultReplaceResponseType());

            yield break;
        }
        /// <summary> 
        /// This Handler triggers whenever is a request for GET on
        /// the service web browser. We can handle any operation
        /// we want for noticing there has been a Get Request on the web.
        /// </summary>
        /// <param name="get">the get request</param>
        /// <returns></returns>
        [ServiceHandler(ServiceHandlerBehavior.Concurrent)]
        public IEnumerator<ITask> HttpGetHandler(HttpGet get)
        {
            //Add here the actions in response to the web browser notification             

            get.ResponsePort.Post(new HttpResponseType(HttpStatusCode.OK, _state));

            yield break;
        }
        #endregion

        #region Embedded Simulation Handlers
        void InsertEntityNotificationHandlerFirstTime(InsertSimulationEntity ins)
        {
            _bitmapPort = new Port<Bitmap>();

            InsertEntityNotificationHandler(ins);
            Activate(Arbiter.Interleave(
                    new TeardownReceiverGroup
                    (
                    ),
                    new ExclusiveReceiverGroup(
                        Arbiter.Receive<FromWinformMsg>(true, _fromWinformPort, OnWinformMessageHandler),
                        Arbiter.Receive<InsertSimulationEntity>(true, _notificationTarget, InsertEntityNotificationHandler),
                        Arbiter.Receive<DeleteSimulationEntity>(true, _notificationTarget, DeleteEntityNotificationHandler),
                        Arbiter.Receive<Bitmap>(true, _bitmapPort, UpdateCameraImage)
                    ),
                    new ConcurrentReceiverGroup()
                ));
        }
        void InsertEntityNotificationHandler(InsertSimulationEntity ins)
        {
            _observer = (CameraEntity)ins.Body;
            _observer.Subscribe(_bitmapPort);
        }
        void DeleteEntityNotificationHandler(DeleteSimulationEntity del)
        {
            _observer = null;
        }
        void UpdateCameraImage(Bitmap bmp)
        {
            WinFormsServicePort.Post(new FormInvoke(delegate() { mainUI.SetCameraImage(bmp); }));
        }
        #endregion
        
        // process messages from the UI Form
        void OnWinformMessageHandler(FromWinformMsg msg)
        {
            #region MSG Switch
            switch (msg.Command)
            {
                case FromWinformMsg.MsgEnum.Loaded:
                    // the windows form is ready to go
                    mainUI = (MainForm)msg.Object;
                    WinFormsServicePort.FormInvoke(delegate() { mainUI.SetHeadless(_defaultConfig.Headless); });
                    break;

                case FromWinformMsg.MsgEnum.Drag:
                    if (_observer != null)
                    {
                        Vector2 drag = (Vector2)msg.Object;
                        xnaTypes.Vector3 view = _observer.LookAt - _observer.Location;
                        view.Normalize();
                        xnaTypes.Vector3 up = new xnaTypes.Vector3(0, 1, 0);
                        float dot = xnaTypes.Vector3.Dot(view, up);
                        if (Math.Abs(dot) > 0.99)
                        {
                            up += new xnaTypes.Vector3(0.1f, 0, 0);
                            up.Normalize();
                        }
                        xnaTypes.Vector3 right = xnaTypes.Vector3.Cross(view, up);
                        view = xnaTypes.Vector3.Multiply(view, 10f);
                        view = xnaTypes.Vector3.Transform(view, xnaTypes.Matrix.CreateFromAxisAngle(up, (float)(-drag.X * Math.PI / 500)));
                        view = xnaTypes.Vector3.Transform(view, xnaTypes.Matrix.CreateFromAxisAngle(right, (float)(-drag.Y * Math.PI / 500)));

                        _observer.LookAt = _observer.Location + view;
                    }
                    break;

                case FromWinformMsg.MsgEnum.Zoom:
                    if (_observer != null)
                    {
                        Vector2 drag = (Vector2)msg.Object;
                        xnaTypes.Vector3 view = _observer.LookAt - _observer.Location;
                        view.Normalize();
                        view = xnaTypes.Vector3.Multiply(view, drag.X * 0.1f);
                        _observer.LookAt += view;
                        _observer.Location += view;
                    }
                    break;
                case FromWinformMsg.MsgEnum.Close:
                    Console.ForegroundColor = ConsoleColor.Red;
                    Console.WriteLine("Press Ctrl+C for terminating service");
                    break;
                case FromWinformMsg.MsgEnum.ConsoleTest:
                    Console.WriteLine("Emergency Stop Pressed");
                    break;
            }
            #endregion
        }

        //ONE JAGUAR PREVIOUS CODE
        //public void NotifySimJaguarHandler(simjaguar.Replace replace)
        //{
        //    _state.SimJaguar.Pose.X_pose = replace.Body.Jaguar.Pose.X_pose;
        //    _state.SimJaguar.Pose.Y_pose = replace.Body.Jaguar.Pose.Y_pose;
        //    _state.SimJaguar.Pose.Z_pose = replace.Body.Jaguar.Pose.Z_pose;

        //    _state.SimJaguar.Pose.Roll = -replace.Body.Jaguar.Pose.OrientationVector.Z * 180 / Math.PI;
        //    _state.SimJaguar.Pose.Pitch = -replace.Body.Jaguar.Pose.OrientationVector.X * 180 / Math.PI;
        //    _state.SimJaguar.Pose.Yaw = replace.Body.Jaguar.Pose.Heading;

        //    _state.SimJaguar.Pose.Speed = replace.Body.Jaguar.Pose.Speed;
        //    _state.SimJaguar.Pose.LeftFrontSpeed = replace.Body.Jaguar.Pose.LeftFrontSpeed;
        //    _state.SimJaguar.Pose.LeftRearSpeed = replace.Body.Jaguar.Pose.LeftRearSpeed;
        //    _state.SimJaguar.Pose.RightFrontSpeed = replace.Body.Jaguar.Pose.RightFrontSpeed;
        //    _state.SimJaguar.Pose.RightRearSpeed = replace.Body.Jaguar.Pose.RightRearSpeed;

        //    if (replace.Body.Jaguar.Sensors.laserData != null &&
        //        replace.Body.Jaguar.Sensors.WebcamBitmap != null &&
        //        replace.Body.Jaguar.Sensors.WebcamData != null &&
        //        mainUI != null)
        //    {
        //        _state.SimJaguar.Sensors.laserData = replace.Body.Jaguar.Sensors.laserData;
        //        _state.SimJaguar.Sensors.WebcamBitmap = replace.Body.Jaguar.Sensors.WebcamBitmap;
        //        _state.SimJaguar.Sensors.WebcamData = replace.Body.Jaguar.Sensors.WebcamData;
        //        //HERE WE MUST CALL FOR AUTONOMOUS OPERATIONS SINCE THIS IS UPDATE AT THE LASER RATE, WHICH IS THE FASTEST.
        //        UpdateMainUI();
        //    }

        //}

        public void NotifySimJaguarsHandler(simjaguars.Replace replace)
        {
            Spawn(replace,UpdateState); // --> This includes the UpdateUI(), which includes UpdateCommands() in which
            //WE MUST CALL FOR AUTONOMOUS OPERATIONS SINCE THIS IS UPDATE AT THE LASER RATE, WHICH IS THE FASTEST.            
        }
        
        #endregion

        void UpdateState(simjaguars.Replace replace)
        {
            for (int i = 1; i < 4; i++)
            {
                if (replace.Body.jaguar[i] != null)
                {
                    _state.SimJaguar[i].Pose.X_pose = replace.Body.jaguar[i].Pose.X_pose;
                    _state.SimJaguar[i].Pose.Y_pose = replace.Body.jaguar[i].Pose.Y_pose;
                    _state.SimJaguar[i].Pose.Z_pose = replace.Body.jaguar[i].Pose.Z_pose;

                    _state.SimJaguar[i].Pose.Roll = -replace.Body.jaguar[i].Pose.OrientationVector.Z * 180 / Math.PI;
                    _state.SimJaguar[i].Pose.Pitch = -replace.Body.jaguar[i].Pose.OrientationVector.X * 180 / Math.PI;
                    _state.SimJaguar[i].Pose.Yaw = replace.Body.jaguar[i].Pose.Heading;

                    _state.SimJaguar[i].Pose.Speed = replace.Body.jaguar[i].Pose.Speed;
                    _state.SimJaguar[i].Pose.LeftFrontSpeed = replace.Body.jaguar[i].Pose.LeftFrontSpeed;
                    _state.SimJaguar[i].Pose.LeftRearSpeed = replace.Body.jaguar[i].Pose.LeftRearSpeed;
                    _state.SimJaguar[i].Pose.RightFrontSpeed = replace.Body.jaguar[i].Pose.RightFrontSpeed;
                    _state.SimJaguar[i].Pose.RightRearSpeed = replace.Body.jaguar[i].Pose.RightRearSpeed;

                    _state.SimJaguar[i].Sensors.laserData = replace.Body.jaguar[i].Sensors.laserData;
                    _state.SimJaguar[i].Sensors.WebcamBitmap = replace.Body.jaguar[i].Sensors.WebcamBitmap;
                    _state.SimJaguar[i].Sensors.WebcamData = replace.Body.jaguar[i].Sensors.WebcamData;

                    Spawn(UpdateMainUI);
                }
                else
                    Console.WriteLine("null jaguar : " + i);
            }
        }
        void UpdateMainUI()
        {
            if (firstTime)
            {
                loadIMUFigures();
                firstTime = false;
            }

            Fault fault = null;
            FormInvoke setImage = new FormInvoke(
                delegate()
                {
                    if (mainUI != null)
                    {
                        //MainUI operations
                        mainUI.sSimPoseX.Text = _state.SimJaguar[1].Pose.X_pose.ToString("0.0");
                        mainUI.sSimPoseY.Text = _state.SimJaguar[1].Pose.Y_pose.ToString("0.0");
                        mainUI.sSimPoseZ.Text = _state.SimJaguar[1].Pose.Z_pose.ToString("0.0");
                        mainUI.sSimYaw.Text = _state.SimJaguar[1].Pose.Yaw.ToString("0.0");
                        mainUI.sSimPitch.Text = _state.SimJaguar[1].Pose.Pitch.ToString("0.0");
                        mainUI.sSimRoll.Text = _state.SimJaguar[1].Pose.Roll.ToString("0.0");
                        mainUI.textBoxYaw.Text = _state.SimJaguar[1].Pose.Yaw.ToString("0.0");
                        mainUI.textBoxPitch.Text = _state.SimJaguar[1].Pose.Pitch.ToString("0.0");
                        mainUI.textBoxRoll.Text = _state.SimJaguar[1].Pose.Roll.ToString("0.0");
                        if (mainUI.SimJaguar1CamDisplay.Checked)
                            mainUI.sSimJaguarCamera.Image = _state.SimJaguar[1].Sensors.WebcamBitmap;
                        if (mainUI.SimJaguar1LaserDisplay.Checked)
                            mainUI.sDrawLaser(_state.SimJaguar[1].Sensors.laserData.DistanceMeasurements, 1);

                        mainUI.sSimPoseX2.Text = _state.SimJaguar[2].Pose.X_pose.ToString("0.0");
                        mainUI.sSimPoseY2.Text = _state.SimJaguar[2].Pose.Y_pose.ToString("0.0");
                        mainUI.sSimPoseZ2.Text = _state.SimJaguar[2].Pose.Z_pose.ToString("0.0");
                        mainUI.sSimYaw2.Text = _state.SimJaguar[2].Pose.Yaw.ToString("0.0");
                        mainUI.sSimPitch2.Text = _state.SimJaguar[2].Pose.Pitch.ToString("0.0");
                        mainUI.sSimRoll2.Text = _state.SimJaguar[2].Pose.Roll.ToString("0.0");
                        mainUI.textBoxYaw2.Text = _state.SimJaguar[2].Pose.Yaw.ToString("0.0");
                        mainUI.textBoxPitch2.Text = _state.SimJaguar[2].Pose.Pitch.ToString("0.0");
                        mainUI.textBoxRoll2.Text = _state.SimJaguar[2].Pose.Roll.ToString("0.0");
                        if (mainUI.SimJaguar2CamDisplay.Checked)
                            mainUI.sSimJaguarCamera2.Image = _state.SimJaguar[2].Sensors.WebcamBitmap;
                        if (mainUI.SimJaguar2LaserDisplay.Checked)
                            mainUI.sDrawLaser(_state.SimJaguar[2].Sensors.laserData.DistanceMeasurements, 2);

                        mainUI.sSimPoseX3.Text = _state.SimJaguar[3].Pose.X_pose.ToString("0.0");
                        mainUI.sSimPoseY3.Text = _state.SimJaguar[3].Pose.Y_pose.ToString("0.0");
                        mainUI.sSimPoseZ3.Text = _state.SimJaguar[3].Pose.Z_pose.ToString("0.0");
                        mainUI.sSimYaw3.Text = _state.SimJaguar[3].Pose.Yaw.ToString("0.0");
                        mainUI.sSimPitch3.Text = _state.SimJaguar[3].Pose.Pitch.ToString("0.0");
                        mainUI.sSimRoll3.Text = _state.SimJaguar[3].Pose.Roll.ToString("0.0");
                        mainUI.textBoxYaw3.Text = _state.SimJaguar[3].Pose.Yaw.ToString("0.0");
                        mainUI.textBoxPitch3.Text = _state.SimJaguar[3].Pose.Pitch.ToString("0.0");
                        mainUI.textBoxRoll3.Text = _state.SimJaguar[3].Pose.Roll.ToString("0.0");
                        if (mainUI.SimJaguar3CamDisplay.Checked)
                            mainUI.sSimJaguarCamera3.Image = _state.SimJaguar[3].Sensors.WebcamBitmap;
                        if (mainUI.SimJaguar3LaserDisplay.Checked)
                            mainUI.sDrawLaser(_state.SimJaguar[3].Sensors.laserData.DistanceMeasurements, 3);

                        SpawnIterator(UpdateMap);
                        UpdateWaypoints();
                        UpdateIMUFigures();
                        UpdateCommands();
                    }
                }
            );

            WinFormsServicePort.Post(setImage);
            Arbiter.Choice(
                setImage.ResultPort,
                delegate(EmptyValue success) { },
                delegate(Exception e)
                {
                    fault = Fault.FromException(e);
                }
            );

            if (fault != null)
            {
                LogError(null, "Unable to update Main UI");
                Console.ForegroundColor = ConsoleColor.Red;
                Console.WriteLine("Unable to update Main UI");
                Console.ForegroundColor = ConsoleColor.Gray;
            }
        }
        public void UpdateCommands()
        {
            if (mainUI != null)
            {
                #region ONE JAGUAR PREVIOUS CODE
                ////COMMAND TO THE SIMULATED ROBOT
                //Speed = new simjaguar.UpdateSuccessMsgRequest();
                //Speed.LeftSpeed = (double)mainUI.sLeftWheelsSpeedTrackBar.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar.Value / 100.0 + (double)mainUI.sSteeringSpeedTrackBar.Value / 100.0;
                //Speed.RightSpeed = (double)mainUI.sRightWheelsSpeedTrackBar.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar.Value / 100.0 - (double)mainUI.sSteeringSpeedTrackBar.Value / 100.0;
                //_simjaguarPort.UpdateSuccessMsg(Speed);
                #endregion 

                if (mainUI.manualcontrol)
                {
                    Speed = new simjaguars.UpdateSuccessMsgRequest();
                    Speed.robotID = 1;
                    Speed.LeftSpeed = (double)mainUI.sLeftWheelsSpeedTrackBar.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar.Value / 100.0 + (double)mainUI.sSteeringSpeedTrackBar.Value / 100.0;
                    Speed.RightSpeed = (double)mainUI.sRightWheelsSpeedTrackBar.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar.Value / 100.0 - (double)mainUI.sSteeringSpeedTrackBar.Value / 100.0;
                    _simjaguarsPort.UpdateSuccessMsg(Speed);
                }
                if (mainUI.manualcontrol2)
                {
                    Speed.robotID = 2;
                    Speed.LeftSpeed = (double)mainUI.sLeftWheelsSpeedTrackBar2.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar2.Value / 100.0 + (double)mainUI.sSteeringSpeedTrackBar2.Value / 100.0;
                    Speed.RightSpeed = (double)mainUI.sRightWheelsSpeedTrackBar2.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar2.Value / 100.0 - (double)mainUI.sSteeringSpeedTrackBar2.Value / 100.0;
                    _simjaguarsPort.UpdateSuccessMsg(Speed);
                }
                if (mainUI.manualcontrol3)
                {
                    Speed.robotID = 3;
                    Speed.LeftSpeed = (double)mainUI.sLeftWheelsSpeedTrackBar3.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar3.Value / 100.0 + (double)mainUI.sSteeringSpeedTrackBar3.Value / 100.0;
                    Speed.RightSpeed = (double)mainUI.sRightWheelsSpeedTrackBar3.Value / 100.0 + (double)mainUI.sDrivingSpeedTrackBar3.Value / 100.0 - (double)mainUI.sSteeringSpeedTrackBar3.Value / 100.0;
                    _simjaguarsPort.UpdateSuccessMsg(Speed);
                }

                if (mainUI.releasecontrol)
                {

                }
                if (mainUI.releasecontrol2)
                {

                }
                if (mainUI.releasecontrol3)
                {

                }
            }
        }
       

        #region Helpers
        private double CalculateDistance(double x, double y, int robotID)
        {
            double distance;
            distance = Math.Sqrt(Math.Pow(x - _state.SimJaguar[robotID].Pose.X_pose, 2) + Math.Pow(y - _state.SimJaguar[robotID].Pose.Z_pose, 2));
            return distance;
        }
        private string GetStringWithUniquePostfix(string name)
        {
            int count;
            if (_stringToIntMapping.TryGetValue(name, out count))
            {
                _stringToIntMapping[name] = count + 1;
            }
            else
            {
                _stringToIntMapping[name] = 1;
            }
            return name + "_" + (count + 1).ToString();
        }
        #endregion
    }
}


